﻿#pragma once

#include "robot_common.h"
#include "path_plan.h"
#include "HandEyeCal.h"
//pcl
#include <pcl\point_types.h>
#include <pcl\point_cloud.h>
#include <pcl\search\octree.h>
//cv
#include <opencv2\imgproc.hpp>

class  Robot
{
public:
	typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
	typedef PointCloud::Ptr   PointCloudPtr;
public:
	bool
		camera2Robot(std::vector<cv::Point3f>& centerPoints, std::vector<cv::Mat>& coordinates, std::vector<LocationDH>& locations);
	//相机坐标系转机器人坐标系
	//		flag  =  0  eye to hand
	//      flag  =  1  eye in hand 
	 bool
		camera2Robot(LocationDH camera_pt, LocationDH& robot_pt, int falg = 0);
	 bool
		camera2Robot(std::vector<LocationDH> camera_pts, std::vector<LocationDH>& robot_pts,int flag = 0);
	 bool
		 setEnvirCloud(PointCloud& input_cloud);
	 bool
		 setScanCloud(PointCloud& input_cloud);
	 bool
		 setOriLoc(LocationDH& ori) { input_ori_ = ori; return true; }
	bool
		loadConfigFile(std::string file_path);
	bool getDestLoc(std::vector<LocationDH> input_dest, std::vector<LocationDH>& output);
private:
	Eigen::Matrix4f eyetohand_mat_;
	std::vector<Eigen::Matrix3f> eyetohand_Rs_;
	Eigen::Matrix4f eyeinhand_mat_;
	LocationDH input_ori_;
private:
	PathPlan path_plan_;
};



